/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008. All Rights Reserved.                             */
/* Open Source Software - may be modified and shared by FRC teams. The code   */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project.                                                               */
/*----------------------------------------------------------------------------*/
package com.nurdrobotics;

import edu.wpi.first.wpilibj.*;
import edu.wpi.first.wpilibj.can.CANTimeoutException;

/**
 * 
 */
public class Motor {
    // The following variables give names to each CAN bus ID to correspond
    // to which motor that jaguar is connected to.
    // This way, it's easy to look at this section of code to make sure that
    // the Jaguars are wired corretly and have the correct IDs.

    public static final int ID_DRIVE_FRONT_LEFT = 2;
    public static final int ID_DRIVE_REAR_LEFT = 3;
    public static final int ID_DRIVE_FRONT_RIGHT = 4;
    public static final int ID_DRIVE_REAR_RIGHT = 5;
    public static final int ID_LIFTER = 6;
    public static final int ID_STAGER_VERTICAL = 7;
    public static final int ID_SHOOTER_LEFT1 = 8;
    public static final int ID_SHOOTER_LEFT2 = 9;
    public static final int ID_SHOOTER_RIGHT1 = 10;
    public static final int ID_SHOOTER_RIGHT2 = 11;
    public static final int ID_SHOOTER_PITCH = 12;
    // delcare the maximum CAN bus ID that exists
    private static final int ID_MAX = 16;
    // declare an array for holding all of the motors
    private static SpeedController m_motorArray[] = null;

    /*
     * This function returns a SpeedController that corresponds to the specific
     * CAN bus ID.
     * An example of how to use this function is as follows. If you want to get
     * the front left motor, you would say:
     *
     * SpeedController motor = Motor.GetMotor(Motor.ID_FRONT_LEFT);
     *
     */
    public static SpeedController getInstance(int motorId) {
        // initialize the motor array if it is not already created
        if (m_motorArray == null) {
            // create an array for holding all the motors
            // Remember the first index in the array is 0, the second is one, etc.
            // So the index to the last motor in the array is ID_MAX - 1
            m_motorArray = new SpeedController[ID_MAX];
            for (int i = 0; i < ID_MAX; i++) {
                m_motorArray[i] = null;
            }
        }

        // get an index into the motor array from the supplied motor ID
        // the array index is equal to the the motor ID - 1
        // So ID 1 is at index 0, ID 2 is at index 1, etc.
        int index = motorId - 1;

        if (m_motorArray[index] == null) {
            try {
                CANJaguar jaguar = new CANJaguar(motorId);
                jaguar.setSafetyEnabled(false);
                m_motorArray[index] = jaguar;
            } catch (CANTimeoutException ex) {
                Dashboard.setErrorStatus("Init Motor " + motorId);
                Dashboard.update();
                ex.printStackTrace();
            }
        }

        return m_motorArray[index];
    }
}
